package com.gitee.wsl.mathematics.geometry.d3

import com.gitee.wsl.mathematics.coordinate.d3.Coordinate3F
import com.gitee.wsl.mathematics.coordinate.d3.Point3F
import com.gitee.wsl.mathematics.matrix.mat4.Mat4f
import com.gitee.wsl.mathematics.vector.ext.dot
import com.gitee.wsl.mathematics.vector.ext.plusAssign
import com.gitee.wsl.mathematics.vector.update
import com.gitee.wsl.mathematics.vector.vec3.MutableVec3f
import com.gitee.wsl.mathematics.vector.vec3.Vec3f
import kotlin.math.sqrt

/**
 * @author Colton Daily
 * @date 11/23/2021
 */
class Ray3d {
    val origin = MutableVec3f()
    val direction = MutableVec3f()

    fun set(other: Ray3d) {
        origin.set(other.origin)
        direction.set(other.direction)
    }

    fun setFromLookAt(origin: Vec3f, lookAt: Vec3f) {
        this.origin.set(origin)
        direction.update {
            set(lookAt)
            subtract(origin)
            norm()
        }
    }

    fun distanceToPoint(point: Vec3f): Float = point.distanceToRay(origin, direction)

    fun sqrDistanceToPoint(point: Vec3f): Float = point.sqrDistanceToRay(origin, direction)

    fun sqrDistanceToPoint(x: Float, y: Float, z: Float) =
        sqrDistancePointToRay(x, y, z, origin, direction)

    fun sphereIntersection(center: Vec3f, radius: Float, result: MutableVec3f): Boolean {
        result.update{
            set(origin)
            subtract(center)
        }
        val a = direction dot direction
        val b = result dot direction * 2f
        val c = (result dot result) - radius * radius
        val discr = b * b - 4 * a * c

        if (discr < 0f) {
            return false
        }

        val numerator = -b - sqrt(discr)
        if (numerator > 0f) {
            val d = numerator / (2f * a)
            result.update {
                set(direction)
                scale(d)
                plusAssign(origin)
            }
            return true
        }

        val numerator2 = -b + sqrt(discr)
        if (numerator2 > 0f) {
            val d = numerator2 / (2f * a)
            result.update {
                set(direction)
                scale(d)
                plusAssign(origin)
            }
            return true
        }

        return false
    }

    fun transformBy(matrix: Mat4f) {

        //matrix.transform(origin)
        //matrix.transform(direction, 0f).norm()
    }

    override fun toString(): String {
        return "{origin=$origin, direction=$direction}"
    }
}



fun Coordinate3F<*>.distanceToRay(ray3d: Ray3d) = distanceToRay(ray3d.origin, ray3d.direction)

fun Coordinate3F<*>.distanceToRay(origin: Point3F, direction: Point3F) = sqrt(sqrDistanceToRay(origin, direction))

fun Coordinate3F<*>.sqrDistanceToRay(ray: Ray3d) = sqrDistanceToRay(ray.origin, ray.direction)

fun Coordinate3F<*>.sqrDistanceToRay(origin: Point3F, direction:Point3F) =
    sqrDistancePointToRay(x, y, z, origin, direction)

fun sqrDistancePointToRay(x: Float, y: Float, z: Float, origin: Point3F, direction: Point3F): Float {
    val nx: Float
    val ny: Float
    val nz: Float
    val dot = x * direction.x + y * direction.y + z * direction.z
    val l = (dot - (origin dot direction)) / (direction dot direction)
    if (l <= 0) {
        nx = origin.x - x
        ny = origin.y - y
        nz = origin.z - z
    } else {
        nx = direction.x * l + origin.x - x
        ny = direction.y * l + origin.y - y
        nz = direction.z * l + origin.z - z
    }
    return nx * nx + ny * ny + nz * nz
}

fun Coordinate3F<*>.nearestPointOnRay(origin: Point3F, direction: Point3F, result: MutableVec3f): MutableVec3f {
    val l = (dot(direction) - (origin dot direction)) / (direction dot direction)
    return if (l <= 0) {
        result.update { set(origin)}
    } else {
        result.update { set(direction)
            scale(l)
            plusAssign(origin)
        }
    }
}